#!/usr/bin/env python

import rospy
from visualization_msgs.msg import Marker
from visualization_msgs.msg import MarkerArray
from geometry_msgs.msg import Point
from sensor_msgs.msg import Image
import math
import numpy as np
from cv_bridge import CvBridge, CvBridgeError  # 确保这里正确导入了CvBridge
import cv2
from std_msgs.msg import String
from mavros_msgs.msg import GPSRAW
import json  # 导入json模块来解析字符串格式的JSON

uav_position = Point(x=1.2, y=2.2, z=3.3)

class Visualization:
    def __init__(self, frame_id="base_link"):
        self.markers_pub = rospy.Publisher('/markers', MarkerArray, queue_size=10)
        self.markers = MarkerArray()
        self.frame_id = frame_id
        
        # self.paint_circle()
        self.paint_text()
        # self.paint_line()
        # print_uav_state()

    def paint_circle(self):
        for circle_id in range(10):
            circle = Marker()

            # meta information
            circle.header.frame_id = self.frame_id
            circle.header.stamp = rospy.Time.now()
            circle.ns = "circle"
            circle.id = circle_id
            circle.action = Marker.ADD
            circle.type = Marker.LINE_STRIP
            circle.lifetime = rospy.Duration(0)
            # geometry information
            self.set_orientation(circle)
            for theta in np.arange(0, 2 * 3.24, 0.1):
                point = Point()
                point.x = circle_id * 10 * math.cos(theta)
                point.y = circle_id * 10 * math.sin(theta)
                point.z = -2
                circle.points.append(point)
            # color information
            self.set_color(circle, (1, 1, 1, 0.9))
            # style information
            circle.scale.x = 0.1  # line width

            self.markers.markers.append(circle)

    # def paint_text(self):
    #     for text_id in range(10):
    #         text = Marker()

    #         # meta information
    #         text.header.frame_id = self.frame_id
    #         text.header.stamp = rospy.Time.now()
    #         text.ns = "text"
    #         text.id = text_id
    #         text.action = Marker.ADD
    #         text.type = Marker.TEXT_VIEW_FACING
    #         text.lifetime = rospy.Duration(0)
    #         # geometry information
    #         theta = -45 * math.pi / 180
    #         text.pose.position.x = (text_id * 10) * math.cos(theta)
    #         text.pose.position.y = (text_id * 10) * math.sin(theta)
    #         text.pose.position.z = 0
    #         # color information
    #         self.set_color(text, (1, 1, 1, 0.9))
    #         # style information
    #         text.scale.z = 2.0  # font size
    #         text.text = f"{text_id * 10}m"

    #         self.markers.markers.append(text)
    
    def paint_text(self):
        # 假设uav_position是一个包含无人机位置信息的Point对象，格式为[x, y, z]
        # 我们将在无人机位置的正上方1米处显示文本

        # X坐标文本
        text_x = Marker()
        text_x.header.frame_id = self.frame_id
        text_x.header.stamp = rospy.Time.now()
        text_x.ns = "uav_position"
        text_x.id = 1  # 用于区分不同的文本标记
        text_x.action = Marker.ADD
        text_x.type = Marker.TEXT_VIEW_FACING
        text_x.lifetime = rospy.Duration(0)
        text_x.pose.position.x = 2
        text_x.pose.position.y = 0
        text_x.pose.position.z = 0  # 将文本设置在无人机正上方1米的位置
        self.set_color(text_x, (1, 0, 0, 0.9))  # 红色文本
        text_x.scale.z = 1.0  # 字体大小
        text_x.text = f"X: {uav_position.x}m"

        self.markers.markers.append(text_x)  # 添加到MarkerArray

        # Y坐标文本
        text_y = Marker()
        text_y.header.frame_id = self.frame_id
        text_y.header.stamp = rospy.Time.now()
        text_y.ns = "uav_position"
        text_y.id = 2
        text_y.action = Marker.ADD
        text_y.type = Marker.TEXT_VIEW_FACING
        text_y.lifetime = rospy.Duration(0)
        text_y.pose.position.x = 4
        text_y.pose.position.y = 0  # 稍微偏移y坐标以区分不同的文本
        text_y.pose.position.z = 0
        self.set_color(text_y, (0, 1, 0, 0.9))  # 绿色文本
        text_y.scale.z = 1.0
        text_y.text = f"Y: {uav_position.y}m"

        self.markers.markers.append(text_y)

        # Z坐标文本
        text_z = Marker()
        text_z.header.frame_id = self.frame_id
        text_z.header.stamp = rospy.Time.now()
        text_z.ns = "uav_position"
        text_z.id = 3
        text_z.action = Marker.ADD
        text_z.type = Marker.TEXT_VIEW_FACING
        text_z.lifetime = rospy.Duration(0)
        text_z.pose.position.x = 6
        text_z.pose.position.y = 0  # 进一步偏移y坐标
        text_z.pose.position.z = 0
        self.set_color(text_z, (0, 0, 1, 0.9))  # 蓝色文本
        text_z.scale.z = 1.0
        text_z.text = f"Z: {uav_position.z}m"

        self.markers.markers.append(text_z)

        
 
    def paint_line(self):
        line = Marker()

        # meta information
        line.header.frame_id = self.frame_id
        line.header.stamp = rospy.Time.now()
        line.ns = "line"
        line.id = 500
        line.action = Marker.ADD
        line.type = Marker.LINE_STRIP
        line.lifetime = rospy.Duration(0)
        # geometry information
        self.set_orientation(line)
        line.points.append(self.create_point([0, 0, -2]))
        line.points.append(self.create_point([100, 0, -2]))
        # color information
        self.set_color(line, (1, 1, 1, 0.9))
        # style information
        line.scale.x = 0.1  # line width

        self.markers.markers.append(line)

    def create_point(self, position):
        p = Point()
        p.x = position[0]
        p.y = position[1]
        p.z = position[2]
        return p

    def set_color(self, marker, rgba):
        marker.color.r = rgba[0]
        marker.color.g = rgba[1]
        marker.color.b = rgba[2]
        marker.color.a = rgba[3]  # Required, otherwise rviz can not be displayed

    def set_orientation(self, marker):
        marker.pose.orientation.x = 0.0  # suppress the uninitialized quaternion, assuming identity warning
        marker.pose.orientation.y = 0.0
        marker.pose.orientation.z = 0.0
        marker.pose.orientation.w = 1.0

    def run(self):
        r = rospy.Rate(10)
        while not rospy.is_shutdown():
            self.markers_pub.publish(self.markers)
            r.sleep()


class ImageProcessor:
    def __init__(self, input_topic, output_topic):
        self.bridge = CvBridge()
        self.image_sub = rospy.Subscriber(input_topic, Image, self.image_callback)
        self.image_pub = rospy.Publisher(output_topic, Image, queue_size=10)

    def image_callback(self, data):
        try:
            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
        except CvBridgeError as e:
            rospy.logerr("CvBridge Error: {0}".format(e))
            return

        # 在图像上添加OSD
        self.add_osd(cv_image, "Processed Image")

        # 将带有OSD的图像发布
        try:
            ros_image = self.bridge.cv2_to_imgmsg(cv_image, "bgr8")
            ros_image.header.stamp = rospy.Time.now()
            self.image_pub.publish(ros_image)
        except CvBridgeError as e:
            rospy.logerr("CvBridge Error: {0}".format(e))

    def add_osd(self, image, text):
        # 在图像上添加OSD文本
        font = cv2.FONT_HERSHEY_SIMPLEX
        cv2.putText(image, text, (180, 30), font, 0.4, (0, 0, 0), 1, cv2.LINE_AA)

    def run(self):
        rospy.spin()
    
class Dji:
    def __init__(self, input_topic, output_topic):
        self.dji_sub = rospy.Subscriber(input_topic, String, self.dji_callback)
        self.dji_pub = rospy.Publisher(output_topic, GPSRAW, queue_size=10)
        self.timer = rospy.Timer(rospy.Duration(1.0), self.timer_callback, oneshot=False)
        self.geo_location_msg = GPSRAW()
    def timer_callback(self, event):
        # 发布消息
        self.dji_pub.publish(self.geo_location_msg)
        rospy.loginfo("Timer callback triggered, message published.")

    def dji_callback(self, data):
        # 解析数据
        # 尝试将字符串格式的JSON解析为字典
        data_dict = json.loads(data.data)  # 注意data.data，因为data是rospy.msg.String类型的对象
        # 现在data_dict是一个字典，你可以通过键来访问数据
        latitude = data_dict['data']['latitude']
        longitude = data_dict['data']['longitude']
        # 转换经纬度为十万分之一度的整数形式
        lat_int = int(latitude * 1e7)
        lon_int = int(longitude * 1e7)  
        # 创建GeoLocation消息
        self.geo_location_msg.header.stamp = rospy.Time.now()

        self.geo_location_msg.lat = lat_int
        self.geo_location_msg.lon = lon_int
        
        # rospy.loginfo("Published GeoLocation: Lat: %f, Long: %f", latitude, longitude)
    
    def run(self):
        rospy.spin()

def main():
    rospy.init_node('rviz_decorator', anonymous=False)
    # 实例化ImageProcessor类
    # image_processor = ImageProcessor("/iris_0/usb_cam/image_raw", "/iris_0/image_osd")
    image_processor = ImageProcessor("/uav1/rtsp_camera_relay/image", "/uav1/image_osd")
    dji = Dji("/pong/primitive", "/dji/gps")

    count = 0
        # vis = Visualization()
        # vis.run()
    count += 1 
    rospy.loginfo('count:%d', count)
    # image_processor.run()
    dji.run()


if __name__ == '__main__':
    main()

